
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_wpnav.h
  * @author     baiyang
  * @date       2022-3-10
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <ahrs/ahrs_view.h>
#include <common/gp_defines.h>
#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
#include <mc_position_control/mc_position_control.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
// get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
enum WPNavTerrainSource {
    WPNAV_TERRAIN_UNAVAILABLE,
    WPNAV_TERRAIN_FROM_RANGEFINDER,
    WPNAV_TERRAIN_FROM_TERRAINDATABASE,
};

/** @ 
  * @brief  
  */
typedef struct {
    ahrs_view*     _ahrs;
    Position_ctrl* _pos_control;

    // parameters
    Param_float    _wp_speed_cms;          // default maximum horizontal speed in cm/s during missions
    Param_float    _wp_speed_up_cms;       // default maximum climb rate in cm/s
    Param_float    _wp_speed_down_cms;     // default maximum descent rate in cm/s
    Param_float    _wp_radius_cm;          // distance from a waypoint in cm that, when crossed, indicates the wp has been reached
    Param_float    _wp_accel_cmss;         // horizontal acceleration in cm/s/s during missions
    Param_float    _wp_accel_z_cmss;       // vertical acceleration in cm/s/s during missions
    Param_float    _wp_jerk;               // maximum jerk used to generate scurve trajectories in m/s/s/s
    Param_float    _terrain_margin;        // terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin

    // terrain following variables
    bool           _terrain_alt;   // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin
    bool           _rangefinder_available; // true if rangefinder is enabled (user switch can turn this true/false)
    Param_int8     _rangefinder_use;       // parameter that specifies if the range finder should be used for terrain following commands
    bool           _rangefinder_healthy;   // true if rangefinder distance is healthy (i.e. between min and maximum)
    float          _rangefinder_alt_cm;    // latest distance from the rangefinder
} mc_wpnav;
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void wpnav_ctor(mc_wpnav* wpnav, ahrs_view* ahrs, Position_ctrl* pos_control);

/// get default target horizontal velocity during wp navigation
static inline float wpnav_get_default_speed_xy(mc_wpnav* wpnav) { return wpnav->_wp_speed_cms; }

/// get default target climb speed in cm/s during missions
static inline float wpnav_get_default_speed_up(mc_wpnav* wpnav) { return wpnav->_wp_speed_up_cms; }

/// get default target descent rate in cm/s during missions.  Note: always positive
static inline float wpnav_get_default_speed_down(mc_wpnav* wpnav) { return fabsf(wpnav->_wp_speed_down_cms); }

/// get_speed_z - returns target descent speed in cm/s during missions.  Note: always positive
static inline float wpnav_get_accel_z(mc_wpnav* wpnav) { return wpnav->_wp_accel_z_cmss; }

/// get_wp_acceleration - returns acceleration in cm/s/s during missions
static inline float wpnav_get_wp_acceleration(mc_wpnav* wpnav) { return wpnav->_wp_accel_cmss; }

static inline float wpnav_get_wp_radius_cm(mc_wpnav* wpnav) { return wpnav->_wp_radius_cm; }

// get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
enum WPNavTerrainSource wpnav_get_terrain_source(mc_wpnav* wpnav);

// get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude)
bool wpnav_get_terrain_offset(mc_wpnav* wpnav, float* offset_cm);

// convert location to vector from ekf origin.  terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain
//      returns false if conversion failed (likely because terrain data was not available)
bool wpnav_get_vector_NEU(const Location *loc, Vector3f_t *vec, bool *terrain_alt);

// return terrain following altitude margin.  vehicle will stop if distance from target altitude is larger than this margin
static inline float wpnav_get_terrain_margin(mc_wpnav* wpnav) { return MAX(wpnav->_terrain_margin, 0.1f); }

// return true if range finder may be used for terrain following
static inline bool wpnav_rangefinder_used(mc_wpnav* wpnav) { return wpnav->_rangefinder_use; }
static inline bool wpnav_rangefinder_used_and_healthy(mc_wpnav* wpnav) { return wpnav->_rangefinder_use && wpnav->_rangefinder_healthy; }

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



